package ch.areion.areionapp.engine;

import ch.areion.areionapp.communication.Monitor;
import ch.areion.areionapp.core.Disconnectable;
import ch.areion.common.message.Module;
import ch.areion.common.message.Type;
import ch.areion.common.message.Version;
import ioio.lib.api.exception.ConnectionLostException;
import ioio.lib.api.IOIO;
import ioio.lib.api.AnalogInput;
import ioio.lib.api.TwiMaster;

import static ch.areion.areionapp.engine.MotorDriver.*;
import static ch.areion.areionapp.engine.MotorDriver.Register.*;

/**
 * Thread to control battery voltage.
 * @author Reto Mueller
 * @version 1.0
 */
public class MotorControl implements Runnable, Disconnectable
{
	private Thread thread;
	private Engine engine;
	private IOIO ioio;
	private AnalogInput analogInM1;
	private AnalogInput analogInM2;
	private TwiMaster iic;
	private float currentM1; //[mA]
	private float currentM2; //[mA]
	private static boolean die = false;
	private static final int IOIO_PORT_1 = 40;
	private static final int IOIO_PORT_2 = 41;
	private static final float RESISTOR_SENSE = 0.5f; // [Ohm]
	private static final float CURRENT_MAX = 2500f; //[mA]

	private static final long UPDATE_PERIODE = 500; // [ms]
	private static final boolean UPDATE_SPEED = false;
	private static final boolean UPDATE_CURRENT = false;
	private static final boolean UPDATE_TICKS = false;
	private static final boolean OVERCURRENT_STOP = false;


	/**
	 * Constructor of BatteryControl
	 * @param ioio_ makes it possible to connect with IOIO
	 * @param iic_ iic connection
	 * @exception ConnectionLostException
	 */
	public MotorControl(Engine engine_, IOIO ioio_, TwiMaster iic_) throws ConnectionLostException
	{
		engine = engine_;
		ioio = ioio_;
		iic = iic_;
		analogInM1= ioio.openAnalogInput(IOIO_PORT_1);
		analogInM2 = ioio.openAnalogInput(IOIO_PORT_2);
	}

	/**
	 * Starts the Thread.
	 */
	public void start()
	{
		if(thread == null){
			thread = new Thread(this);
			thread.start();
		}
	}

	/**
	 * Disconnects the BatteryControl from IOIO.
	 */
	public void disconnect() throws InterruptedException
	{
		die = true;
		thread.join();
		analogInM1.close();
		analogInM2.close();
	}

	/**
	 * Motor control loop. Checks current and speed periodically and reports critical changes.
	 */
	public void run()
	{
		while(!die){
			try{
				if(UPDATE_SPEED){
					getSpeed();
				}
				if(UPDATE_CURRENT){
					getCurrent();
				}
				if(UPDATE_TICKS){
					getTicks();
				}				
				Thread.sleep(UPDATE_PERIODE);
			}catch(ConnectionLostException c){
				//Monitor.getInstance().add(Version.V01, Type.ERROR, Module.OTHER, c.getMessage()); No need to report, is reported in the main Controller class.
			}catch(InterruptedException e){
				Monitor.getInstance().add(Version.V01, Type.ERROR, Module.OTHER, e.getMessage());
			}			
		}
	}

	/**
	 * Checks current and reports critical changes.
	 * @exception ConnectionLostException
	 * @exception InterruptedException
	 */
	private void getCurrent() throws ConnectionLostException, InterruptedException
	{
		currentM1 = (analogInM1.getVoltage()*1000)/RESISTOR_SENSE;
		currentM2 = (analogInM2.getVoltage()*1000)/RESISTOR_SENSE;
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.ENGINE, "Motor 1 current ", currentM1 + " mA");
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.ENGINE, "Motor 2 current ", currentM2 + " mA");
		if(currentM1 >= CURRENT_MAX){
			Monitor.getInstance().add(Version.V01, Type.WARNING, Module.OTHER, "Motor 1 current reaches critical level.");
			if(OVERCURRENT_STOP){
				engine.stop();
			}
		}else if(currentM2 >= CURRENT_MAX){
			Monitor.getInstance().add(Version.V01, Type.WARNING, Module.OTHER, "Motor 2 current reaches critical level.");
			if(OVERCURRENT_STOP){
				engine.stop();
			}
		}
	}

	/**
	 * Checks speed.
	 * @exception ConnectionLostException
	 * @exception InterruptedException
	 */
	private void getSpeed() throws ConnectionLostException, InterruptedException
	{
		byte[] response = new byte[4];
		iic.writeRead(SLAVE_ADR, false, M1_SPEED_HB.cmd(), 1, response, response.length);

		Monitor.getInstance().add(Version.V01, Type.INFO, Module.ENGINE, "Motor 1 speed ", toShort(response, 0) / SPEED_CORRECTION +" mm/s");
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.ENGINE, "Motor 2 speed ", toShort(response, 2) / SPEED_CORRECTION +" mm/s");
	}

	/**
	 * Get the ticks of the encoder.
	 * @exception ConnectionLostException
	 * @exception InterruptedException
	 */
	private void getTicks() throws ConnectionLostException, InterruptedException
	{
		byte[] response = new byte[4];
		iic.writeRead(SLAVE_ADR, false, M1_ENC_TICKS_HB.cmd(), 1, response, response.length);

		Monitor.getInstance().add(Version.V01, Type.INFO, Module.ENGINE, "Motor 1 encoder ticks   ", (toShort(response, 0) & 0xFFFF)+ "");
		Monitor.getInstance().add(Version.V01, Type.INFO, Module.ENGINE, "Motor 2 encoder ticks   ", (toShort(response, 2) & 0xFFFF)+"");
	}
}
